#include "TrajectoryGenerator.h"
#include <iostream>
#include <string>

using namespace std;
using namespace Eigen;

TrajectoryGenerator::TrajectoryGenerator()
        : D_(Eigen::Vector<double, 13 * PREDICTION_HORIZON>()),
            setflag_(0),
            k_(0),
            block_para_(0.5),
            v_k_com_d_O_(Vector3d()),
            p_k_com_d_O_(Vector3d()),
            omega_k_d_O_(Vector3d()),
            THETA_k_d_(Vector3d()),
            p_0_com_d_O_(Vector3d()),
            v_0_com_d_O_(Vector3d()),
            p_0_com_O_(Vector3d())
{
}

TrajectoryGenerator::~TrajectoryGenerator()
{
}

void TrajectoryGenerator::setDesired(const double &v_x, const double &v_y, const double &omega_z)
{
    // 设置期望值
    v_x_d_B_ = v_x;
    v_y_d_B_ = v_y;
    omega_z_d_B_ = omega_z;

    setflag_ = 1;
    // TODO:检测输入不变时不重复计算
}

void TrajectoryGenerator::calculateAll(const Matrix3d &Rot)
{
    // 确保已设置输入
    if (setflag_ != 1)
    {
        cerr << "TrajectoryGenerator: set expection before get result!"<<endl;
        exit(1);
    }

    int p_h = PREDICTION_HORIZON;

    // 初始化 v_1_com_d_O -> D_({9,10,11})
    Vector3d v_k_com_d_B;
    v_k_com_d_B << v_x_d_B_, v_y_d_B_, 0;

    double omega_k_z_d_O = omega_z_d_B_;
    double psi_0_d = atan(Rot(1, 0) / Rot(0, 0));
    double psi_1_d = psi_0_d + omega_k_z_d_O * DELTA_T_MPC;

    D_({9, 10, 11}) = R("z", psi_1_d) * v_k_com_d_B;

    // 初始化 p_1_com_d_O_ -> D_({3,4,5})
    Vector3d p_1_com_d_O;
    p_1_com_d_O = block_para_ * p_0_com_d_O_ + (1 - block_para_) * p_0_com_O_ + v_0_com_d_O_ * DELTA_T_MPC;

    D_({3, 4, 5}) = p_1_com_d_O;

    // 初始化 omega_1_d_O_ -> D_({6,7,8})
    D_({6, 7, 8}) << 0, 0, omega_k_z_d_O;

    // 初始化 THETA_1_d_ -> D_({0,1,2})

    // TODO: (*D_)({0, 1, 2}) << 0, 0, omega_k_z_d_O;
    D_({0, 1, 2}) << 0, 0, omega_k_z_d_O;

    double psi_k_d = psi_0_d;
    for (int i = 1; i < p_h; i++)
    {
        // 计算2~h周期所有v_k_com_d_O_
        psi_k_d = psi_k_d + omega_k_z_d_O * DELTA_T_MPC;
        D_({9 + 13 * i, 10 + 13 * i, 11 + 13 * i}) = R("z", psi_k_d) * v_k_com_d_B;

        // 计算2~h周期所有p_k_com_d_O_
        D_({3 + 13 * i, 4 + 13 * i, 5 + 13 * i}) =
            D_({3 + 13 * (i - 1), 4 + 13 * (i - 1), 5 + 13 * (i - 1)}) +
            D_({9 + 13 * (i - 1), 10 + 13 * (i - 1), 11 + 13 * (i - 1)}) * DELTA_T_MPC;

        // 计算2~h周期所有omega_k_d_O_
        D_({6 + 13 * i, 7 + 13 * i, 8 + 13 * i}) << 0, 0, omega_k_z_d_O;

        //TODO: 计算2~h周期所有THETA_k_d_
        D_({0 + 13 * i, 1 + 13 * i, 2 + 13 * i}) << 0, 0, omega_k_z_d_O;
    }
}

void TrajectoryGenerator::getD(Vector<double, 13 * PREDICTION_HORIZON> &vectorForD)
{
    vectorForD = D_;
}

// Vector3d *TrajectoryGenerator::getv_k_com_d_O(int k, Matrix3d *Rot)
// {
//     //确保已设置输入
//     if (setflag_ != 1)
//     {
//         cerr << "Expected speed not set!";
//         exit(1);
//     }

//     double psi_0_d = atan(((*Rot)(1, 0)) / ((*Rot)(0, 0)));
//     double psi_k_d = 0.0;
//     double omega_k_z_d_O = omega_z_d_B_;

//     for (int i = k_; i < k; i++)
//     {
//         psi_k_d = psi_k_d + omega_k_z_d_O * DELTA_T_MPC;
//     }

//     Vector3d v_k_com_d_B;
//     v_k_com_d_B << v_x_d_B_, v_y_d_B_, 0;

//     Matrix3d tmpmat;
//     tmpmat.setIdentity();
//     (*v_k_com_d_O_) = R"z", psi_k_d) * v_k_com_d_B;

//     return v_k_com_d_O_;
// }

// Vector3d *TrajectoryGenerator::getp_k_com_d_O(int k, Vector3d *p_0_com_O, Matrix3d *Rot)
// {

// }
